import cv2
import numpy as np
import imutils
import serial
import time

height = 480
width = 640
vertex = []
temp = []
send_data = []
send_len = 0

ser = serial.Serial("/devyAMA0", 9600)


def CRC16_Check(datas, lens):
    CRC16 = 0xffff
    for i in range(0, lens):
        d = int(datas[i])
        CRC16 = CRC16 ^ d
        print(CRC16)
        for i in range(0, 8):
            state = CRC16 & 0x01
            CRC16 >>= 1
            if state != 0:
                CRC16 = CRC16 ^ 0xA001
    return hex(CRC16)


def send_message():
    global send_data, send_len
    print(send_data)
    high_x = send_data[0] / 100
    low_x = send_data[0] % 100
    high_y = send_data[1] / 100
    low_y = send_data[1] % 100
    if send_data[0] >= 0:
        qaq_x = 0
    elif send_data[0] < 0:
        qaq_x = 1
    if send_data[1] >= 0:
        qaq_y = 0
    elif send_data[1] < 0:
        qaq_y = 1
    send_data[0] = qaq_x
    send_data[1] = high_x
    send_data.append(low_x)
    send_data.append(qaq_y)
    send_data.append(high_y)
    send_data.append(low_y)
    CRC16 = CRC16_Check(send_data, send_len)
    print(CRC16)
    CRC1 = int(CRC16, 16) >> 8
    CRC2 = int(CRC16, 16) & 0xff
    print(CRC1)
    print(CRC2)
    # FH=bytearray([0xA5,0x5A,send_len,0x01,int(qaq_x),int(high_x),int(low_x),int(qaq_y),int(high_y),int(low_y),CRC1,CRC2,0xff])
    FH1 = bytearray([0xA5, 0x5A])
    FH2 = bytearray([send_len, 0x02])
    FH3 = bytearray([int(qaq_x), int(high_x)])
    FH4 = bytearray([int(low_x), int(qaq_y)])
    FH5 = bytearray([int(high_y), int(low_y)])
    FH6 = bytearray([CRC1, CRC2])
    ser.write(FH1)
    time.sleep(0.05)
    ser.write(FH2)
    time.sleep(0.05)
    ser.write(FH3)
    time.sleep(0.05)
    ser.write(FH4)
    time.sleep(0.05)
    ser.write(FH5)
    time.sleep(0.05)
    ser.write(FH6)
    send_data.clear()


def send_pos(cx, cy):
    global send_data, send_len
    send_data.append(cx)
    send_data.append(cy)
    send_len = 6
    send_message()


def laser_location(img):
    grid_RGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

    # 从RGB色彩空间转换到HSV色彩空间
    grid_HSV = cv2.cvtColor(grid_RGB, cv2.COLOR_RGB2HSV)

    # H、S、V范围一：
    lower1 = np.array([0, 43, 46])
    upper1 = np.array([10, 255, 255])
    mask1 = cv2.inRange(grid_HSV, lower1, upper1)  # mask1 为二值图像
    res1 = cv2.bitwise_and(grid_RGB, grid_RGB, mask=mask1)

    # H、S、V范围二：
    lower2 = np.array([156, 43, 46])
    upper2 = np.array([180, 255, 255])
    mask2 = cv2.inRange(grid_HSV, lower2, upper2)
    res2 = cv2.bitwise_and(grid_RGB, grid_RGB, mask=mask2)

    # 将两个二值图像结果 相加
    mask3 = mask1 + mask2
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
    dilated = cv2.dilate(mask3, kernel)
    erode = cv2.erode(dilated, kernel)
    contours, hierarchy = cv2.findContours(erode, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    cv2.imshow("maxk", erode)
    contour_f = []
    for contour in contours:
        #
        contour_f.append(contour)
    for con in contour_f:
        cv2.drawContours(img, [con], -1, (255, 0, 0), 1)
        center, (width, height), angle = cv2.minAreaRect(con)
        return center
    return None


def rect_location(img):
    global temp
    boxs = []
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    edges = cv2.Canny(gray, 100, 200)
    contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    for contour in contours:
        center, (width, height), angle = cv2.minAreaRect(contour)
        rect = cv2.minAreaRect(contour)
        if width * height >= 39000 and width <= 50000:
            box = np.intp(cv2.boxPoints(rect))
            boxs.append(box)
            cv2.drawContours(img, [contour], -1, (0, 0, 255), 1)
    temp = boxs
    if len(boxs) != 0:
        if len(boxs) == 4:
            return np.intp((boxs[0] + boxs[2]) / 2)
        elif len(boxs) == 2:
            return np.intp((boxs[0] + boxs[1]) / 2)
        else:
            return None
    return None


def if_close(x, y):
    return x * x + y * y < 400


def task2(cap):
    state = 0
    divx = 100
    divy = 100
    vertexlist = []
    for i in range(5):
        diff = (vertex[1] - vertex[0]) / 5
        vertexlist.append(vertex[0] + diff * i)
    for i in range(5):
        diff = (vertex[2] - vertex[1]) / 5
        vertexlist.append((vertex[1] + diff * i))
    for i in range(5):
        diff = (vertex[3] - vertex[2]) / 5
        vertexlist.append((vertex[2] + diff * i))
    for i in range(5):
        diff = (vertex[0] - vertex[3]) / 5
        vertexlist.append((vertex[3] + diff * i))
    vertexlist = np.intp(vertexlist)
    for i in range(len(vertexlist)):
        while True:
            ret, frame = cap.read()
            cv2.imshow("ori", frame)
            print("state:", state)
            print("divx:", divx)
            print("divy:", divy)
            center = laser_location(frame)
            if center is not None:
                cv2.circle(frame, (vertexlist[i][0], vertexlist[i][1]), 10, (0, 255, 0), 3)
                divx = -(vertexlist[i][0] - center[0])
                divy = -(vertexlist[i][1] - center[1])
                send_pos(cx,cy)
                cv2.imshow("e", frame)
                if if_close(divx, divy):
                    break
            cv2.waitKey(1)


def task1(cap):
    state = 0
    divx = 100
    divy = 100
    while True:
        print("state:", state)
        print("divx:", divx)
        print("divy:", divy)
        ret, frame = cap.read()
        cv2.imshow("ori", frame)
        center = laser_location(frame)
        # 顺时针的
        if center is not None and len(vertex) != 0:
            # cx = width - center[1]
            # cy = height - center[0]
            # center = (cx, cy)
            # for r in rect:
            #     cx = width - r[1]
            #     cy = height - r[0]
            #     r[0] = cx
            #     r[1] = cy
            # 阶段1：左上-右上
            if state == 0:
                cv2.circle(frame, (vertex[0][0], vertex[0][1]), 10, (0, 255, 0), 3)
                divx = -(vertex[0][0] - center[0])
                divy = -(vertex[0][1] - center[1])
                send_pos(divx,divy)
                if if_close(divx, divy):
                    state = 1
            # 阶段2：右上-右下
            elif state == 1:
                cv2.circle(frame, (vertex[1][0], vertex[1][1]), 10, (0, 255, 0), 3)
                divx = -(vertex[1][0] - center[0])
                divy = -(vertex[1][1] - center[1])
                send_pos(divx, divy)
                if if_close(divx, divy):
                    state = 2
            # 阶段3：右下-左下
            elif state == 2:
                cv2.circle(frame, (vertex[2][0], vertex[2][1]), 10, (0, 255, 0), 3)
                divx = -(vertex[2][0] - center[0])
                divy = -(vertex[2][1] - center[1])
                send_pos(divx, divy)
                if if_close(divx, divy):
                    state = 3
            # 阶段4：左下-左上
            elif state == 3:
                cv2.circle(frame, (vertex[3][0], vertex[3][1]), 10, (0, 255, 0), 3)
                divx = -(vertex[3][0] - center[0])
                divy = -(vertex[3][1] - center[1])
                send_pos(divx, divy)
                if if_close(divx, divy):
                    state = 4
            elif state == 4:
                break
        cv2.imshow("e", frame)
        cv2.waitKey(10)


# box = np.int0(cv2.boxPoints(rect))
def main():
    global vertex
    cap = cv2.VideoCapture(0)
    while True:
        print("in while")
        ret, frame = cap.read()
        box = rect_location(frame)
        if box is not None:
            for b in box:
                vertex = box
            break
    task2(cap)
    print(temp)
    print(vertex)


main()
